/**********************************************************************************************************************
 * @file    gnss_info_process.c
 * @author  Queclink Jason.huang
 * @date    2019-01-03
 * @brief   About the processing of GNSS positioning information.
 *
 * Copyright (C) 2018 Queclink Wireless Solutions (ShenZhen) Co., Ltd. All Rights Reserved.
 *
 * @attention
 *
 * @usage
 *
 *********************************************************************************************************************/

/* includes ---------------------------------------------------------------------------------------------------------*/
#include "aadl/aadl.h"
#include "gnss_info_process.h"
#include <gnss_public.h>
#include <qlcommon.h>
#include <gnss/nmea/nmea.h>
#include "mtk_series01.h"
#include "gnss_log.h"
#include "bsp/inc/bsp_gnss.h"

#define GNSS_ACC_MAX        (10)            /* 设备最大加速度 1g(≈10m/s) */

/* 处理后的定位信息 */
struct fix_info gnss_fix_info       = {0};
double          gnss_last_stderr    = 0;
unsigned int    gnss_last_fix_time  = 0;
static bool     gnss_weak_signal    = false;       /* 只是当前是否弱信号, 卫星信息更新时同时更新状态 */

/*******************************************************************************
* 函数名称：gnss_pwron_discard
* 功能描述: 丢弃刚启动时，x秒内的定位数据
* 输入参数：
* 返 回 值：bool 返回true时，表示正在执行丢点操作，返回flase,表示丢点操作结束。
* 作    者:
* 完成日期: 2019-01-03
******************************************************************************/
bool gnss_pwron_discard(anti_drift_manage_t * anti_drift_ptr,struct fix_info *info)
{
    bool ret = false;

    if (FIX_OK == info->state){
        /* 从上电后的首次定位开始计时 */
        if (0 == anti_drift_ptr->fix_delay.fix_delay_time)
            anti_drift_ptr->fix_delay.fix_delay_time = getms();
        if(!timeout_ms(anti_drift_ptr->fix_delay.fix_delay_time, 5 *1000)){
            /*开始定位的几个点不可靠，丢弃*/
            anti_drift_ptr->fix_delay.no_fix_cnt = 0;
            gnss_trace(LOG_DEBUG"First positioning filter.{cnt:%d}\r\n", 5);
            ret = true;
        }
        /*若发生连续不定位事件，若定位时接收到的是2D点，丢弃前10个 */
        if (anti_drift_ptr->fix_delay.no_fix_cnt >= 5 &&
            anti_drift_ptr->cnt_2d_fix > 0 && anti_drift_ptr->cnt_2d_fix <= 10){
            gnss_trace(LOG_DEBUG"discard 2d fix after a succession of no fix.\r\n");
            anti_drift_ptr->fix_delay.no_fix_cnt = 0;
            ret = true;
        }
        anti_drift_ptr->fix_delay.no_fix_cnt = 0;
    }else{
        anti_drift_ptr->fix_delay.no_fix_cnt++;
    }

    return ret;
}


/*******************************************************************************
* 功能描述: 检测是否缺失速度，航向值
* 输入参数：info 本次的定位数据
* 返 回 值：true:速度缺失且未曾获得速度值，需要强制将此点改为未定位
* 作    者:
* 完成日期:
******************************************************************************/
bool gnss_speed_absence(struct fix_info *info)
{
    static float last_speed, last_angle;
    gnss_task_manage_t *gps_device = get_gnss_manage();

    if (gps_device->gnss_info.speed_is_absent){
        /*速度缺失，使用曾获取的速度值*/
        if (gps_device->gnss_info.has_get_speed){
            info->speed = last_speed;
        }else{
            /*速度缺失且未曾获得速度值，需要强制将此点改为未定位*/
            gnss_trace(LOG_INFO"Speed information is absent and never get speed information before.\r\n");
            return true;
        }
    }else{
        last_speed = info->speed;
        gps_device->gnss_info.has_get_speed = true;
    }

    if (gps_device->gnss_info.angle_is_absent){
        /*航向缺失，使用曾获取的航向值*/
        if (gps_device->gnss_info.has_get_angle){
            info->angle = last_angle;
        }
    }else{
        last_angle = info->angle;
        gps_device->gnss_info.has_get_angle = true;
   }
   return false;
}

/*******************************************************************************
* 功能描述: 检测UTC时间是否异常，过滤定位异常的点
* 输入参数：info 本次的定位数据
* 返 回 值：true:UTC时间异常，需要强制将此点改为未定位
* 作    者:
* 完成日期:
******************************************************************************/
void gnss_utc_filter(struct fix_info *info)
{
    static unsigned int utc_bak = 0, sec_bak = 0;
    static unsigned char err_cnt = 0;
    unsigned int utc_new = 0, sec_new = 0;
    unsigned int utc_diff = -1, sec_diff = -1;
    bool need_dection = true;

    if (FIX_OK != info->state)
        return;

    info->utc.tm_isdst = -1;
    utc_new = mktime(&(info->utc));
    sec_new = getsec();

    /* 首次不检查 */
    if (0 == utc_bak && 0 == sec_bak)
        need_dection = false;

    /* 本地RTC倒退了，或者计数器溢出了(最多计40多天)，则本次不检查 */
    if (sec_new < sec_bak)
        need_dection = false;
    else
        sec_diff = sec_new - sec_bak;

    if (utc_new >= utc_bak)
        utc_diff = utc_new - utc_bak;

    if (need_dection) {
        if (((utc_new < utc_bak) || (abs(utc_diff - sec_diff) > 604800)) &&
            err_cnt++ < 10) {
            info->state = FIX_INVALID;
            gnss_trace(LOG_WARN"UTC error event occures. { ticks_diff=%d,utc_diff=%d}\r\n",
                                            sec_diff,utc_diff);
            mtkSeries01_Reboot(GNSS_HOT_START);
            return;
        }
        /* 多次重启模块后仍异常，认为当前utc正常 */
    }

    /* 异常次数累计重置 */
    err_cnt = 0;
    utc_bak = utc_new;
    sec_bak = sec_new;
}


static bool gnss_speed_discard(anti_drift_manage_t * anti_drift_ptr, struct fix_info *info)
{
    double          distance = 0;
    unsigned int    current_time = getms();
    double          diff_time = 0;
    bool            discard = false;

    double          avg_speed = 0;                      /* 当前平均速度 */
    double          current_speed = 0;                  /* 当前速度 */

    if (info->speed >= QLSDK_GNSS_MAX_SPEED) {          /* 定位数据的速度异常,直接丢弃 */
        anti_drift_ptr->dist_discard_cnt = 0;
        return true;
    }

    if (0 == anti_drift_ptr->ref_point.lat && 0 == anti_drift_ptr->ref_point.log)
        goto update_ref_point;

    /* 计算当前定位点相对参考点的速度 */
    distance = 1000.0*qlsdk_gnss_calculate_distance(anti_drift_ptr->ref_point.lat, anti_drift_ptr->ref_point.log,
                                    info->latitude, info->longitude);
    diff_time = (current_time - anti_drift_ptr->ref_time)/1000.0;   /* ms to sec */
    if (diff_time <= 0)
        goto update_ref_point;
    avg_speed = distance/diff_time;

    /* 计算相对上一点的速度 */
    distance = 1000.0*qlsdk_gnss_calculate_distance(anti_drift_ptr->last_point.lat, anti_drift_ptr->last_point.log,
                                    info->latitude, info->longitude);
    diff_time = (current_time - anti_drift_ptr->last_time)/1000.0;  /* ms to sec */
    if (diff_time > 0)
        current_speed = distance/diff_time;

    /* 检查速度是否异常 */
    if (avg_speed >= QLSDK_GNSS_MAX_SPEED/3.6)  {               /* 速度异常 */
        if (current_speed >= QLSDK_GNSS_MAX_SPEED/3.6) {
            if (anti_drift_ptr->dist_discard_cnt++ < 0xFF)
                anti_drift_ptr->dist_discard_cnt++;
        } else {
            anti_drift_ptr->dist_discard_cnt = 0;
        }

        if (anti_drift_ptr->dist_discard_cnt <= 5)
            discard = true;
        else
            anti_drift_ptr->dist_discard_cnt = 0;                   /* 接受此点 */
    }


    /* 检查加速度是否正常 */
    if (!discard &&
        abs((int)((current_speed-anti_drift_ptr->last_speed)/(current_time-anti_drift_ptr->last_time))) > GNSS_ACC_MAX)
        discard = true;

update_ref_point:
    if (!discard) {                                     /* 当前定位有效，更新参考点 */
        anti_drift_ptr->ref_point.lat = info->latitude;
        anti_drift_ptr->ref_point.log = info->longitude;
        anti_drift_ptr->ref_time = current_time;
    }

    /* 保存最后一个定位点 */
    anti_drift_ptr->last_point.lat = info->latitude;
    anti_drift_ptr->last_point.log = info->longitude;
    anti_drift_ptr->last_time = current_time;
    anti_drift_ptr->last_speed = avg_speed;

    return discard;
}

/*******************************************************************************
* 函数名称： gnss_info_proc
* 功能描述:  GPS 定位点信息处理函数，用于获取NEMA定位点，并根据配置对定位点做相关操作。
* 输入参数：
* 返 回 值：
* 作    者:
* 完成日期:
******************************************************************************/
void gnss_info_proc(struct fix_info *info)
{
    gnss_task_manage_t *gps_device;
    gps_device = get_gnss_manage();
    nmea_protocol_parse_t *nmea_ptr = get_nmea_manage_context();
    double stderr_ = -1;

    int gsv_cnt = 0;
    for (int i=0; i<SV_MAX; i++)
        gsv_cnt += gps_device->sat[i].count;

    /* 位置误差的标准差 */
    if (nmea_ptr->stderr_.alt >= 0 || nmea_ptr->stderr_.lat >= 0 || nmea_ptr->stderr_.lng >= 0) {
        stderr_ = nmea_ptr->stderr_.alt*nmea_ptr->stderr_.alt\
                    + nmea_ptr->stderr_.lat*nmea_ptr->stderr_.lat\
                    + nmea_ptr->stderr_.lng*nmea_ptr->stderr_.lng;
        stderr_ = sqrt(stderr_);
    }
    nmea_ptr->stderr_.alt = -1;
    nmea_ptr->stderr_.lat = -1;
    nmea_ptr->stderr_.lng = -1;

    /* 可见卫星情况 */
    gnss_trace(LOG_DEBUG "SV: GP-%d GL-%d GA-%d GB-%d\r\n", gps_device->sat[0].count, gps_device->sat[1].count,
                                               gps_device->sat[2].count, gps_device->sat[3].count);

    /* 原始定位信息 */
    memcpy(&gps_device->gnss_info.raw_info, info, sizeof(struct fix_info));
    gps_device->gnss_info.stderr_ = stderr_;

    fix_info_printf("GNSS raw information.              ", &gps_device->gnss_info.raw_info, gsv_cnt, stderr_);

    /* 过滤utc时间异常的定位信息,认为本次定位无效 */
    gnss_utc_filter(info);

    /* 抛弃前x秒的定位信息 */
    if (gnss_pwron_discard(&(gps_device->anti_drift_m),info))
        goto no_fix;

    if (FIX_OK == info->state) {
        /* 信息缺失，定位状态需要置为未定位 */
        if (gnss_speed_absence(info)){
            gnss_trace(LOG_INFO"Because of the lack of part of the information of the positioning data, it is regarded as the unlocated data.\r\n");
            info->state = FIX_INVALID;
            goto no_fix;
        }

        /* 水平精度因子异常, 仅惯导则不过滤 */
        if (info->posmode != POSMODE_E \
            && QLSDK_GNSS_HDOP_THOLD > 0 \
            && info->hdop > QLSDK_GNSS_HDOP_THOLD) {
            if (gps_device->anti_drift_m.hdop_discard_cnt < 10) {
                gps_device->anti_drift_m.hdop_discard_cnt++;
                goto fix_failed;
            }       /* 丢弃 10个后 不丢弃, 低于阈值后重新启动该策略 */
        } else {
            gps_device->anti_drift_m.hdop_discard_cnt = 0;
        }

        /* 速度异常超出阈值丢弃,本次定位无效 */
        if (gnss_speed_discard(&(gps_device->anti_drift_m), info)) {
            gnss_trace(LOG_INFO"This data is discarded due to abnormal speed.\r\n");
            goto fix_failed;
        }
    }

    if (info->state == FIX_OK) {
        gps_device->anti_drift_m.no_fix_num = 0;
        /* 定位成功,更新处理后的定位信息 */
        memcpy(&(gnss_fix_info), (void*)info, sizeof(struct fix_info));
        gnss_last_stderr = stderr_;
        /* 记录获取时间tick */
        gnss_last_fix_time = getsec();
    } else {
fix_failed:     /* 本次定位不成功 */
        if (gps_device->anti_drift_m.no_fix_num < 3)
            gps_device->anti_drift_m.no_fix_num++;
        if (gps_device->anti_drift_m.no_fix_num >= 3) {     /* 多次不定位 */
no_fix:
            gnss_fix_info.state = FIX_INVALID;
        }
    }

    /* 数据更新 */
    fix_info_printf("Optimized positioning information. ", &(gnss_fix_info), gsv_cnt, gnss_last_stderr);

    qltask_gnss_fix_get_hook((const struct fix_info*)(&(gnss_fix_info)), gnss_last_stderr);
}

/* 可见卫星信息处理 */
struct gnss_signal_mgr {
    gnss_signal     signal;         /* 当前信号等级 */
    unsigned char   cnt;

};

static struct gnss_signal_mgr signal_mgr;

void gnss_signal_monitor_init(void)
{
    memset(&signal_mgr, 0, sizeof(struct gnss_signal_mgr));
    signal_mgr.signal = GNSS_SIGNAL_NO;
}

/* 获取卫星信息对应的信号等级
 * info:    卫星信息,不能为NULL;
 * return:  info对应的信号等级 0~3 数值越大信号越强;
 */
static gnss_signal gnss_signal_level(gnss_task_manage_t *mgr)
{
    unsigned char level_tbl[3] = {0,0,0};

    if (NULL == mgr)
        return GNSS_SIGNAL_NO;

    for (int j=0; j<SV_MAX; j++) {
        for (int i=0; i < mgr->sat[i].count; i++) {
            if (mgr->sat[i].sat[i].signal_strength >= 10)
                level_tbl[0]++;
            if (mgr->sat[i].sat[i].signal_strength >= 25)
                level_tbl[1]++;
            if (mgr->sat[i].sat[i].signal_strength >= 20)
                level_tbl[2]++;
        }
    }

    if (level_tbl[2] >= 4)                  /* 需要至少四颗或以上的可见卫星才能有较好的定位效果 */
        return GNSS_SIGNAL_STRONG;
    if (level_tbl[1] >= 3)
        return GNSS_SIGNAL_GOOD;
    if (level_tbl[0] >= 3)
        return GNSS_SIGNAL_WEAK;
    return GNSS_SIGNAL_NO;
}

void gnss_sat_info_update(struct sat_info_single *info, uint8_t sv_index, uint8_t signal_id)
{
    gnss_task_manage_t * gnss_manage = get_gnss_manage();
    gnss_signal current_signal;

    static uint8_t last_index   = 0xFF;         /* 记录最后一次更新的卫星索引 */
    static uint8_t last_signal  = 0xFF;         /* 记录最后一次更新的信号ID */
    int i,j;

    /* 缓存卫星信息 */
    gnss_manage->sat_update_mask |= (0x1<<sv_index);        /* 记录该卫星系统信息更新 */
    if (last_index != sv_index || last_signal == signal_id) {
        /* 清空旧的数据 */
        gnss_manage->sat[sv_index].count = 0;
    }
    last_index  = sv_index;
    last_signal = signal_id;

    /* 追加新的卫星信息 */
    for (i=0; i<info->count; i++) {
        for (j=0; j<gnss_manage->sat[sv_index].count; j++) {
            if (info->sat[i].sv_id == gnss_manage->sat[sv_index].sat[j].sv_id) {
                if (info->sat[i].signal_strength > gnss_manage->sat[sv_index].sat[j].signal_strength)
                    gnss_manage->sat[sv_index].sat[j].signal_strength = info->sat[i].signal_strength;
                break;
            }
        }
        if (j == gnss_manage->sat[sv_index].count && gnss_manage->sat[sv_index].count < VIEW_SV_MAX)
            gnss_manage->sat[sv_index].sat[gnss_manage->sat[sv_index].count++] = info->sat[i];
    }

    /* 信号等级判断 */
    current_signal = gnss_signal_level(gnss_manage);
    if (current_signal != signal_mgr.signal) {
        signal_mgr.cnt++;
        if (signal_mgr.cnt >= 2) {                  /* 信号等级改变 */
            signal_mgr.cnt = 0;
            signal_mgr.signal = current_signal;
        }
    } else {
        signal_mgr.cnt = 0;
    }
}

/* 弱信号处理 */
void gnss_usable_sv_update(struct usable_sv_t *usable_sv)
{
    gnss_task_manage_t * gnss_manage = get_gnss_manage();
    uint8_t sig_tmp[3] = {0,0,0};               /* 记录若干最强信号值 */
    uint8_t idx = 0;

    if (NULL != usable_sv) {
        memcpy(&(gnss_manage->usable_sv), usable_sv, sizeof(struct usable_sv_t));

        if (gnss_manage->usable_sv.cnt < 3) {   /* 少于三颗无法定位 */
            gnss_weak_signal = true;
            return;
        }

        for (int i = 0; i < gnss_manage->usable_sv.cnt; i++)
        { // 可用卫星
            for (int j = 0; j < SV_MAX; j++)
            { // 卫星系统 * 4
                for (int k = 0; k < gnss_manage->sat[j].count; k++)
                { // 具体卫星
                    if (gnss_manage->usable_sv.sv_num[i] == gnss_manage->sat[j].sat[k].sv_id)
                    { // 是否是可用卫星
                        /* 查找记录中信号最差的值 */
                        idx = 0;
                        for (int m = 1; m < 3; m++)
                        {
                            if (sig_tmp[idx] > sig_tmp[m])
                                idx = m;
                        }

                        /* 记录更强的信号值 */
                        if (sig_tmp[idx] < gnss_manage->sat[j].sat[k].signal_strength)
                            sig_tmp[idx] = gnss_manage->sat[j].sat[k].signal_strength;
                    }
                }
            }
        }

        gnss_trace(LOG_DEBUG "Three of the best satellites available:%d,%d,%d.\r\n", sig_tmp[0],sig_tmp[1],sig_tmp[2]);
        gnss_best_signal_set(sig_tmp, 3);
        gnss_weak_signal = ((sig_tmp[0] + sig_tmp[1] + sig_tmp[2]) / 3 < 28);
        return;
    }
    gnss_weak_signal = false;
}

/**
 * @brief 判断GPS是否弱信号
 * @return true 
 * @return false 
 */
bool gnss_signal_is_fine(void)
{
    return !gnss_weak_signal;
}


uint8_t gnss_best_signal[3] = {0};
/**
 * @brief 获取GPS已经记录的信号强度，
 * @param  signal     存放信号强度的数组
 * @param  count      最大值为3
 * @return true 
 * @return false 
 */
void gnss_signal_best_get(uint8_t *signal, uint8_t count)
{
    assert_ptr_ret(signal, false);
    assert_expr_ret(count <= 3, false);

    for (uint8_t i = 0; i < count; i++) 
    {
        signal[i] = gnss_best_signal[i];
    }
}

/**
 * @brief 记录最好的信号值
 * @param  signal           My Param doc
 * @param  count            My Param doc
 */
void gnss_best_signal_set(uint8_t *signal, uint8_t count)
{
    assert_ptr_ret(signal, false);
    assert_expr_ret(count <= 3, false);
    
    for (uint8_t i = 0; i < count; i++) {
        gnss_best_signal[i] = signal[i];
    }   
}
